﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Navigation;
using System.Windows.Shapes;
using System.Windows.Threading;
using Microsoft.Research.DynamicDataDisplay.DataSources;
using Microsoft.Research.DynamicDataDisplay;
using MECopter.Logging;
using MECopter.Quadrocopter;
using MECopter.Configuration.GroundControl;
using MECopter.Configuration.Quadrocopter;
using MECopter.Charts;



namespace MECopter
{
    public partial class MainWindow : Window
    {
        #region Fields
        ScaleTransform scaleTransform;
        private DispatcherTimer refreshTimer;       

        Quadrocopter.Quadrocopter quadrocopter;

        public LoopTimePointCollection looptimePointCollection;
        public AnglePointCollection anglePointCollection;
        public InputPointCollection inputPointCollection;
        public PIDPointCollection pidPointCollection;
        #endregion

        #region Constructor
        public MainWindow()
        {
           
            //Set the main windows titel
            this.Title = "MECopter v" + System.Reflection.Assembly.GetExecutingAssembly().GetName().Version.ToString();

            //Load the general config and decide whether to show the debug console or not
            ConfigurationManager.Load(ConfigType.General);
            if (ConfigurationManager.CurrentGeneralConfig.ShowConsole)
            {
                ConsoleManager.Show();
                Log.Info("General config loaded");

            }
            Log.Info("Initializing");
            InitializeComponent();

            //Load further config files
            ConfigurationManager.Load(ConfigType.Serial);
            ConfigurationManager.Load(ConfigType.Controller);

            //Prepare the LoopTime chart
            looptimePointCollection = new LoopTimePointCollection();
            var dsLoopTime = new EnumerableDataSource<LoopTimePoint>(looptimePointCollection);
            dsLoopTime.SetXMapping(x => loopTime_dateAxis.ConvertToDouble(x.Date));
            dsLoopTime.SetYMapping(y => y.LoopTime);           
            plotter_loopTime.AddLineGraph(dsLoopTime, Colors.Green, 2, "LoopTime");

            //Prepare the Angle chart           
            anglePointCollection = new AnglePointCollection();
            //Yaw
            var dsAngleYaw = new EnumerableDataSource<AnglePoint>(anglePointCollection);
            dsAngleYaw.SetXMapping(x => angle_dateAxis.ConvertToDouble(x.Date));
            dsAngleYaw.SetYMapping(y => y.Angle.Yaw);
            plotter_angle.AddLineGraph(dsAngleYaw, Colors.Red, 1, "Yaw");
            //Pitch
            var dsAnglePitch = new EnumerableDataSource<AnglePoint>(anglePointCollection);
            dsAnglePitch.SetXMapping(x => angle_dateAxis.ConvertToDouble(x.Date));
            dsAnglePitch.SetYMapping(y => y.Angle.Pitch);
            plotter_angle.AddLineGraph(dsAnglePitch, Colors.Green, 1, "Pitch");
            //Roll
            var dsAngleRoll = new EnumerableDataSource<AnglePoint>(anglePointCollection);
            dsAngleRoll.SetXMapping(x => angle_dateAxis.ConvertToDouble(x.Date));
            dsAngleRoll.SetYMapping(y => y.Angle.Roll);
            plotter_angle.AddLineGraph(dsAngleRoll, Colors.Blue, 1, "Roll");


            //Prepare the Input Values chart           
            inputPointCollection = new InputPointCollection();
            //Speed
            var dsInputSpeed = new EnumerableDataSource<InputPoint>(inputPointCollection);
            dsInputSpeed.SetXMapping(x => input_dateAxis.ConvertToDouble(x.Date));
            dsInputSpeed.SetYMapping(y => y.ControllValues.Speed);
            plotter_input.AddLineGraph(dsInputSpeed, Colors.Yellow, 1, "Speed");
            //Yaw
            var dsInputYaw = new EnumerableDataSource<InputPoint>(inputPointCollection);
            dsInputYaw.SetXMapping(x => input_dateAxis.ConvertToDouble(x.Date));
            dsInputYaw.SetYMapping(y => y.ControllValues.Yaw);
            plotter_input.AddLineGraph(dsInputYaw, Colors.Red, 1, "Yaw");
            //Pitch
            var dsInputPitch = new EnumerableDataSource<InputPoint>(inputPointCollection);
            dsInputPitch.SetXMapping(x => input_dateAxis.ConvertToDouble(x.Date));
            dsInputPitch.SetYMapping(y => y.ControllValues.Pitch);
            plotter_input.AddLineGraph(dsInputPitch, Colors.Green, 1, "Pitch");
            //Roll
            var dsInputRoll = new EnumerableDataSource<InputPoint>(inputPointCollection);
            dsInputRoll.SetXMapping(x => input_dateAxis.ConvertToDouble(x.Date));
            dsInputRoll.SetYMapping(y => y.ControllValues.Roll);
            plotter_input.AddLineGraph(dsInputRoll, Colors.Blue, 1, "Roll");


            //Prepare the PID Value chart           
            pidPointCollection = new PIDPointCollection();
            //Yaw
            var dsPIDYaw = new EnumerableDataSource<PIDPoint>(pidPointCollection);
            dsPIDYaw.SetXMapping(x => pid_dateAxis.ConvertToDouble(x.Date));
            dsPIDYaw.SetYMapping(y => y.PIDValues[0]);
            plotter_pid.AddLineGraph(dsPIDYaw, Colors.Red, 1, "Yaw");
            //Pitch
            var dsPIDPitch = new EnumerableDataSource<PIDPoint>(pidPointCollection);
            dsPIDPitch.SetXMapping(x => pid_dateAxis.ConvertToDouble(x.Date));
            dsPIDPitch.SetYMapping(y => y.PIDValues[1]);
            plotter_pid.AddLineGraph(dsPIDPitch, Colors.Green, 1, "Pitch");
            //Roll
            var dsPIDRoll = new EnumerableDataSource<PIDPoint>(pidPointCollection);
            dsPIDRoll.SetXMapping(x => pid_dateAxis.ConvertToDouble(x.Date));
            dsPIDRoll.SetYMapping(y => y.PIDValues[2]);
            plotter_pid.AddLineGraph(dsPIDRoll, Colors.Blue, 1, "Roll");


            //Prepare the Angle prograss bars
            pbAngleYaw.Minimum = -Math.PI / 2;
            pbAngleYaw.Maximum = Math.PI / 2;

            pbAnglePitch.Minimum = -Math.PI / 2;
            pbAnglePitch.Maximum = Math.PI / 2;

            pbAngleRoll.Minimum = -Math.PI / 2;
            pbAngleRoll.Maximum = Math.PI / 2;


            //Prepare the Controll progress bars
            pbCtrlSpeed.Minimum = 0;
            pbCtrlSpeed.Maximum = 2*Math.PI;

            pbCtrlYaw.Minimum = -Math.PI;
            pbCtrlYaw.Maximum = Math.PI;

            pbCtrlPitch.Minimum = -Math.PI;
            pbCtrlPitch.Maximum = Math.PI;

            pbCtrlRoll.Minimum = -Math.PI;
            pbCtrlRoll.Maximum = Math.PI;


            tblSerialStatus.Text = String.Format("Serial: {0} Baudrate {1}  Not connected", ConfigurationManager.CurrentSerialConfig.Port, ConfigurationManager.CurrentSerialConfig.Baudrate);

            //Set parameter for the GUI update timer
            refreshTimer = new DispatcherTimer();
            refreshTimer.Interval = TimeSpan.FromMilliseconds(ConfigurationManager.CurrentGeneralConfig.UIRefreshRate); //TODO: Make this value configurable
            refreshTimer.Tick += refreshTimer_Tick;
        }       
        #endregion

        #region Eventhandler
        private void btn_connect_Click(object sender, RoutedEventArgs e)
        {
            Connect();
        }

        private void btn_disconnect_Click(object sender, RoutedEventArgs e)
        {
            Disconnect();
        }


        private void btn_enableESCs_Click(object sender, RoutedEventArgs e)
        {
            btn_enableESCs.IsEnabled = false;
            btn_disableESCs.IsEnabled = true;
            quadrocopter.EnableESCs();
        }

        private void btn_disableESCs_Click(object sender, RoutedEventArgs e)
        {
            btn_enableESCs.IsEnabled = true;
            btn_disableESCs.IsEnabled = false;
            quadrocopter.DisableESCs();
        }


        private void Connect()
        {
              //Create the Qudrocopter object and enable it
            quadrocopter = new Quadrocopter.Quadrocopter();
            quadrocopter.Enable();

            //Start the GUI update timer   
            if (quadrocopter.IsAvaliable)
            {
                refreshTimer.Start();

                btn_connect.IsEnabled = false;
                btn_disconnect.IsEnabled = true;
                btn_enableESCs.IsEnabled = true;
                topMenuQuadroConnect.IsEnabled = false;
                topMenuQuadroDisconnect.IsEnabled = true;
            }
            else
            {
                Disconnect();
            }
        }

        private void Disconnect()
        {
            refreshTimer.Stop();
            quadrocopter.controller.stopControlling();
            quadrocopter.Disable();

            btn_connect.IsEnabled = true;
            btn_disconnect.IsEnabled = false;
            btn_enableESCs.IsEnabled = false;
            btn_disableESCs.IsEnabled = false;
            topMenuQuadroConnect.IsEnabled = true;
            topMenuQuadroDisconnect.IsEnabled = false;

            tblSerialStatus.Text = String.Format("Serial: {0} Baudrate {1}  Not connected", ConfigurationManager.CurrentSerialConfig.Port, ConfigurationManager.CurrentSerialConfig.Baudrate);
        }

        private void refreshTimer_Tick(object sender, EventArgs e)
        {
            //Input Values
            lbCtrlSpeed.Content = Math.Round(quadrocopter.controller.Values.Speed,6);
            lbCtrlYaw.Content = Math.Round(quadrocopter.controller.Values.Yaw, 6);
            lbCtrlPitch.Content = Math.Round(quadrocopter.controller.Values.Pitch, 6);
            lbCtrlRoll.Content = Math.Round(quadrocopter.controller.Values.Roll, 6);

            try //Try-catch is a kind of hacky right here but it somehow the values get sometimes NaN...
            {
                pbCtrlSpeed.Value = quadrocopter.controller.Values.Speed;
                pbCtrlYaw.Value = quadrocopter.controller.Values.Yaw;
                pbCtrlPitch.Value = quadrocopter.controller.Values.Pitch;
                pbCtrlRoll.Value = quadrocopter.controller.Values.Roll;
            }
            catch(Exception){}

            inputPointCollection.Add(new InputPoint(quadrocopter.controller.Values,DateTime.Now));

            //ESC
            lbESC0.Content = quadrocopter.ESC[0];
            lbESC1.Content = quadrocopter.ESC[1];
            lbESC2.Content = quadrocopter.ESC[2];
            lbESC3.Content = quadrocopter.ESC[3];


            //Angle
            lbYaw.Content = quadrocopter.YPR.Yaw;
            lbPitch.Content = quadrocopter.YPR.Pitch;
            lbRoll.Content = quadrocopter.YPR.Roll;

            pbAngleYaw.Value = quadrocopter.YPR.Yaw;
            pbAnglePitch.Value = quadrocopter.YPR.Pitch;
            pbAngleRoll.Value = quadrocopter.YPR.Roll;

            anglePointCollection.Add(new AnglePoint(quadrocopter.YPR, DateTime.Now));

            //PID Values
            lbPIDYaw.Content = quadrocopter.PIDValues[0];
            lbPIDPitch.Content = quadrocopter.PIDValues[1];
            lbPIDRoll.Content = quadrocopter.PIDValues[2];

            pbPIDYaw.Value = quadrocopter.PIDValues[0];
            pbPIDPitch.Value = quadrocopter.PIDValues[1];
            pbPIDRoll.Value = quadrocopter.PIDValues[2];

            pidPointCollection.Add(new PIDPoint(quadrocopter.PIDValues, DateTime.Now));

            //Loop Time
            lbArduLoopTime.Content = quadrocopter.LoopTime + " µs = " +(int)(quadrocopter.LoopTime / 1000) + "ms";
            looptimePointCollection.Add(new LoopTimePoint(quadrocopter.LoopTime,DateTime.Now));


            //Serial Status
            tblSerialStatus.Text = String.Format("Serial: {0} Baudrate {1}  connected", ConfigurationManager.CurrentSerialConfig.Port, ConfigurationManager.CurrentSerialConfig.Baudrate);
        }

        //Menu
        private void Menu_PC(object sender, RoutedEventArgs e)
        {
            OptionsUI config = new OptionsUI();
            config.Show();
        }

        private void Menu_Quadrocopter(object sender, RoutedEventArgs e)
        {
            QuadroConfigUI config = new QuadroConfigUI();
            config.Show();
        }

        #endregion

        private void Button_Click_1(object sender, RoutedEventArgs e)
        {
            quadrocopter.SetPID(QuadrocopterConfig.pidParameters);
        }

        private void Button_Click_2(object sender, RoutedEventArgs e)
        {
            quadrocopter.GetPID();
        }

        private void Button_Click_3(object sender, RoutedEventArgs e)
        {
            this.WindowStyle = System.Windows.WindowStyle.None;
            this.WindowState = System.Windows.WindowState.Maximized;
            scaleTransform = new ScaleTransform(this.Width / 1280, this.Height / 1024);
            mainGrid.LayoutTransform = scaleTransform;
           
        }

        private void Window_SizeChanged(object sender, SizeChangedEventArgs e)
        {
            
            scaleTransform = new ScaleTransform(this.Width / 1280, this.Height / 1024);
            mainGrid.LayoutTransform = scaleTransform;
        }



    }
}
